﻿using System;
using System.Text;

namespace Microsoft.Xna.Framework
{
    /// <summary>
    /// Defines a frustum and helps determine whether forms intersect with it.
    /// </summary>
    [Serializable]
    public class BoundingFrustum : IEquatable<BoundingFrustum>
    {
        #region Private Fields
        private Plane _bottom;
        private Vector3[] _corners;
        private Plane _far;
        private Plane _left;
        private Matrix _matrix;
        private Plane _near;
        private Plane _right;
        private Plane _top;
        #endregion

        #region Public Fields
        /// <summary>
        /// Gets the bottom plane of the BoundingFrustum.
        /// </summary>
        public Plane Bottom
        {
            get { return _bottom; }
        }
        /// <summary>
        /// Specifies the total number of corners (8) in the BoundingFrustum.
        /// </summary>
        public const int CornerCount = 8;
        /// <summary>
        /// Gets the far plane of the BoundingFrustum.
        /// </summary>
        public Plane Far
        {
            get { return _far; }
        }
        /// <summary>
        /// Gets the left plane of the BoundingFrustum.
        /// </summary>
        public Plane Left
        {
            get { return _left; }
        }
        /// <summary>
        /// Gets or sets the Matrix that describes this bounding frustum.
        /// </summary>
        public Matrix Matrix
        {
            get { return _matrix; } 
            set 
            { 
                _matrix = value; 
                CreatePlanes();    // FIXME: The odds are the planes will be used a lot more often than the matrix 
                CreateCorners();   // is updated, so this should help performance. I hope ;) 
            } 
        }
        /// <summary>
        /// Gets the near plane of the BoundingFrustum.
        /// </summary>
        public Plane Near
        {
            get { return _near; }
        }
        /// <summary>
        /// Gets the right plane of the BoundingFrustum.
        /// </summary>
        public Plane Right
        {
            get { return _right; }
        }
        /// <summary>
        /// Gets the top plane of the BoundingFrustum.
        /// </summary>
        public Plane Top
        {
            get { return _top; }
        }
        #endregion

        #region Constructor
        /// <summary>
        /// Creates a new instance of BoundingFrustum.
        /// </summary>
        /// <param name="value">Combined matrix that usually takes view × projection matrix.</param>
        public BoundingFrustum(Matrix value)
        {
            _matrix = value;
            CreatePlanes();
            CreateCorners();
        } 
        #endregion

        #region Private Methods
        /// <summary>
        /// Helper Method to speed up performance
        /// </summary>
        private void CreateCorners()
        {
            _corners = new Vector3[8];
            _corners[0] = IntersectionPoint(ref _near, ref _left, ref _top);
            _corners[1] = IntersectionPoint(ref _near, ref _right, ref _top);
            _corners[2] = IntersectionPoint(ref _near, ref _right, ref _bottom);
            _corners[3] = IntersectionPoint(ref _near, ref _left, ref _bottom);
            _corners[4] = IntersectionPoint(ref _far, ref _left, ref _top);
            _corners[5] = IntersectionPoint(ref _far, ref _right, ref _top);
            _corners[6] = IntersectionPoint(ref _far, ref _right, ref _bottom);
            _corners[7] = IntersectionPoint(ref _far, ref _left, ref _bottom);
        }
        /// <summary>
        /// Helper Method to speed up performance
        /// </summary>
        private void CreatePlanes()
        {
            // Pre-calculate the different planes needed 
            _left = new Plane(-_matrix.M14 - _matrix.M11, -_matrix.M24 - _matrix.M21,
                                  -_matrix.M34 - _matrix.M31, -_matrix.M44 - _matrix.M41);

            _right = new Plane(_matrix.M11 - _matrix.M14, _matrix.M21 - _matrix.M24,
                                   _matrix.M31 - _matrix.M34, _matrix.M41 - _matrix.M44);

            _top = new Plane(_matrix.M12 - _matrix.M14, _matrix.M22 - _matrix.M24,
                                 _matrix.M32 - _matrix.M34, _matrix.M42 - _matrix.M44);

            _bottom = new Plane(-_matrix.M14 - _matrix.M12, -_matrix.M24 - _matrix.M22,
                                    -_matrix.M34 - _matrix.M32, -_matrix.M44 - _matrix.M42);

            _near = new Plane(-_matrix.M13, -_matrix.M23, -_matrix.M33, -_matrix.M43);


            _far = new Plane(_matrix.M13 - _matrix.M14, _matrix.M23 - _matrix.M24,
                                 _matrix.M33 - _matrix.M34, _matrix.M43 - _matrix.M44);

            _NormalizePlane(ref _left);
            _NormalizePlane(ref _right);
            _NormalizePlane(ref _top);
            _NormalizePlane(ref _bottom);
            _NormalizePlane(ref _near);
            _NormalizePlane(ref _far);
        }
        /// <summary>
        /// Helper method to speed up performance
        /// </summary>
        private static Vector3 IntersectionPoint(ref Plane a, ref Plane b, ref Plane c)
        {
            // Formula used 
            //                d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) 
            //P =       ------------------------------------------------------------------------- 
            //                             N1 . ( N2 * N3 ) 
            // 
            // Note: N refers to the normal, d refers to the displacement. '.' means dot product. '*' means cross product 

            Vector3 v1, v2, v3;
            float f = -Vector3.Dot(a.Normal, Vector3.Cross(b.Normal, c.Normal));

            v1 = (a.D * (Vector3.Cross(b.Normal, c.Normal)));
            v2 = (b.D * (Vector3.Cross(c.Normal, a.Normal)));
            v3 = (c.D * (Vector3.Cross(a.Normal, b.Normal)));

            Vector3 vec = new Vector3(v1.X + v2.X + v3.X, v1.Y + v2.Y + v3.Y, v1.Z + v2.Z + v3.Z);
            return vec / f;
        }
        /// <summary>
        /// Private method to normalize planes.
        /// </summary>
        /// <param name="p">Plane to normalize.</param>
        private void _NormalizePlane(ref Plane p)
        {
            float factor = 1f / p.Normal.Length();
            p.Normal.X *= factor;
            p.Normal.Y *= factor;
            p.Normal.Z *= factor;
            p.D *= factor;
        } 
        #endregion

        #region Public Methods
        /// <summary>
        /// Checks whether the current BoundingFrustum contains the specified BoundingSphere.
        /// </summary>
        /// <param name="sphere">The BoundingSphere to test for overlap.</param>
        /// <param name="result">Enumeration indicating the extent of overlap.</param>
        public void Contains(ref BoundingSphere sphere, out ContainmentType result)
        {
            float val;
            ContainmentType contained;

            // We first check if the sphere is inside the frustum 
            Contains(ref sphere.Center, out contained);

            // The sphere is inside. Now we need to check if it's fully contained or not 
            // So we see if the perpendicular distance to each plane is less than or equal to the sphere's radius. 
            // If the perpendicular distance is less, just return Intersects. 
            if (contained == ContainmentType.Contains)
            {
                val = PlaneHelper.PerpendicularDistance(ref sphere.Center, ref _bottom);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = PlaneHelper.PerpendicularDistance(ref sphere.Center, ref _far);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = PlaneHelper.PerpendicularDistance(ref sphere.Center, ref _left);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = PlaneHelper.PerpendicularDistance(ref sphere.Center, ref _near);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = PlaneHelper.PerpendicularDistance(ref sphere.Center, ref _right);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                val = PlaneHelper.PerpendicularDistance(ref sphere.Center, ref _top);
                if (val < sphere.Radius)
                {
                    result = ContainmentType.Intersects;
                    return;
                }

                // If we get here, the sphere is fully contained 
                result = ContainmentType.Contains;
                return;
            }
            //duff idea : test if all corner is in same side of a plane if yes and outside it is disjoint else intersect 
            // issue is that we can have some times when really close aabb  

            // If we're here, the the sphere's centre was outside of the frustum. This makes things hard :( 
            // We can't use perpendicular distance anymore. I'm not sure how to code _ 
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum contains the specified BoundingSphere.
        /// </summary>
        /// <param name="sphere">The BoundingSphere to test for overlap.</param>
        public ContainmentType Contains(BoundingSphere sphere)
        {
            ContainmentType result;
            Contains(ref sphere, out result);
            return result;
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum contains the specified point.
        /// </summary>
        /// <param name="point">The point to test for overlap.</param>
        /// <param name="result">Enumeration indicating the extent of overlap.</param>
        public void Contains(ref Vector3 point, out ContainmentType result)
        {
            float val;
            // If a point is on the POSITIVE side of the plane, then the point is not contained within the frustum 

            // Check the top 
            val = PlaneHelper.ClassifyPoint(ref point, ref _top);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the bottom 
            val = PlaneHelper.ClassifyPoint(ref point, ref _bottom);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the left 
            val = PlaneHelper.ClassifyPoint(ref point, ref _left);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the right 
            val = PlaneHelper.ClassifyPoint(ref point, ref _right);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the near 
            val = PlaneHelper.ClassifyPoint(ref point, ref _near);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // Check the far 
            val = PlaneHelper.ClassifyPoint(ref point, ref _far);
            if (val > 0)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            // If we get here, it means that the point was on the correct side of each plane to be 
            // contained. Therefore this point is contained 
            result = ContainmentType.Contains;
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum contains the specified point.
        /// </summary>
        /// <param name="point">The point to test for overlap.</param>
        public ContainmentType Contains(Vector3 point)
        {
            ContainmentType result;
            this.Contains(ref point, out result);
            return result;
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum contains the specified BoundingFrustum.
        /// </summary>
        /// <param name="frustum">The BoundingFrustum to check against the current BoundingFrustum.</param>
        /// <remarks>TODO: Implement this </remarks>
        public ContainmentType Contains(BoundingFrustum frustum)
        {
            if (this == frustum)                // We check to see if the two frustums are equal 
                return ContainmentType.Contains;// If they are, there's no need to go any further. 

            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum contains the specified BoundingBox.
        /// </summary>
        /// <param name="box">The BoundingBox to test for overlap.</param>
        /// <param name="result">Enumeration indicating the extent of overlap.</param>
        public void Contains(ref BoundingBox box, out ContainmentType result)
        {
            // FIXME: Is this a bug? 
            // If the bounding box is of W * D * H = 0, then return disjoint 
            if (box.Min == box.Max)
            {
                result = ContainmentType.Disjoint;
                return;
            }

            int i;
            ContainmentType contained;
            Vector3[] corners = box.GetCorners();

            // First we assume completely disjoint. So if we find a point that is contained, we break out of this loop 
            for (i = 0; i < corners.Length; i++)
            {
                this.Contains(ref corners[i], out contained);
                if (contained != ContainmentType.Disjoint)
                    break;
            }

            if (i == corners.Length) // This means we checked all the corners and they were all disjoint 
            {
                result = ContainmentType.Disjoint;
                return;
            }

            if (i != 0)             // if i is not equal to zero, we can fastpath and say that this box intersects 
            {                       // because we know at least one point is outside and one is inside. 
                result = ContainmentType.Intersects;
                return;
            }

            // If we get here, it means the first (and only) point we checked was actually contained in the frustum. 
            // So we assume that all other points will also be contained. If one of the points is disjoint, we can 
            // exit immediately saying that the result is Intersects 
            i++;
            for (; i < corners.Length; i++)
            {
                this.Contains(ref corners[i], out contained);
                if (contained != ContainmentType.Contains)
                {
                    result = ContainmentType.Intersects;
                    return;
                }
            }

            // If we get here, then we know all the points were actually contained, therefore result is Contains 
            result = ContainmentType.Contains;
            return;
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum contains the specified BoundingBox.
        /// </summary>
        /// <param name="box">The BoundingBox to test for overlap.</param>
        public ContainmentType Contains(BoundingBox box)
        {
            ContainmentType result;
            this.Contains(ref box, out result);
            return result;
        }
        /// <summary>
        /// Determines whether the specified Object is equal to the BoundingFrustum.
        /// </summary>
        /// <param name="obj">The object to compare.</param>
        public override bool Equals(object obj)
        {
            BoundingFrustum f = obj as BoundingFrustum;
            return (object.Equals(f, null)) ? false : (this == f);
        }
        /// <summary>
        /// Determines whether the specified BoundingFrustum is equal to the current BoundingFrustum.
        /// </summary>
        /// <param name="other">The BoundingFrustum to compare with the current BoundingFrustum.</param>
        public bool Equals(BoundingFrustum other)
        {
            return (this == other);
        } 
        /// <summary>
        /// Gets an array of points that make up the corners of the BoundingFrustum.
        /// </summary>
        /// <param name="corners">An existing array of at least 8 Vector3 points where the corners of the BoundingFrustum are written.</param>
        public void GetCorners(Vector3[] corners)
        {
            if (corners.Length < _corners.Length)
                throw new ArgumentException("Array must be at least 8 vector points long.");

            corners = GetCorners();
        }
        /// <summary>
        /// Gets an array of points that make up the corners of the BoundingFrustum.
        /// </summary>
        public Vector3[] GetCorners()
        {
            return _corners;
        }
        /// <summary>
        /// Gets the hash code for this instance.
        /// </summary>
        public override int GetHashCode()
        {
            return _matrix.GetHashCode();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects a BoundingSphere.
        /// </summary>
        /// <param name="sphere">The BoundingSphere to check for intersection with.</param>
        /// <param name="result">true if the BoundingFrustum and BoundingSphere intersect; false otherwise.</param>
        public void Intersects(ref BoundingSphere sphere, out bool result)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects a BoundingSphere.
        /// </summary>
        /// <param name="sphere">The BoundingSphere to check for intersection with.</param>
        public bool Intersects(BoundingSphere sphere)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects a Ray.
        /// </summary>
        /// <param name="ray">The Ray to check for intersection with.</param>
        /// <param name="result">Distance at which the ray intersects the BoundingFrustum or null if there is no intersection.</param>
        public void Intersects(ref Ray ray, out Nullable<float> result)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects a Ray.
        /// </summary>
        /// <param name="ray">The Ray to check for intersection with.</param>
        public Nullable<float> Intersects(Ray ray)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects a Plane.
        /// </summary>
        /// <param name="plane">The Plane to check for intersection with.</param>
        /// <param name="result">An enumeration indicating whether the BoundingFrustum intersects the Plane.</param>
        public void Intersects(ref Plane plane, out PlaneIntersectionType result)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects a Plane.
        /// </summary>
        /// <param name="plane">The Plane to check for intersection with.</param>
        public PlaneIntersectionType Intersects(Plane plane)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects the specified BoundingFrustum.
        /// </summary>
        /// <param name="frustum">The BoundingFrustum to check for intersection.</param>
        public bool Intersects(BoundingFrustum frustum)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects a BoundingBox.
        /// </summary>
        /// <param name="box">The BoundingBox to check for intersection with.</param>
        /// <param name="result">true if the BoundingFrustum and BoundingBox intersect; false otherwise.</param>
        public void Intersects(ref BoundingBox box, out bool result)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// Checks whether the current BoundingFrustum intersects a BoundingBox.
        /// </summary>
        /// <param name="box">The BoundingBox to check for intersection with.</param>
        public bool Intersects(BoundingBox box)
        {
            throw new NotImplementedException();
        } 
        /// <summary>
        /// Returns a String that represents the current BoundingFrustum.
        /// </summary>
        public override string ToString()
        {
            StringBuilder sb = new StringBuilder(256);
            sb.Append("{Near:");
            sb.Append(_near.ToString());
            sb.Append(" Far:");
            sb.Append(_far.ToString());
            sb.Append(" Left:");
            sb.Append(_left.ToString());
            sb.Append(" Right:");
            sb.Append(_right.ToString());
            sb.Append(" Top:");
            sb.Append(_top.ToString());
            sb.Append(" Bottom:");
            sb.Append(_bottom.ToString());
            sb.Append("}");
            return sb.ToString();
        } 
        #endregion

        #region Operators
        /// <summary>
        /// Determines whether two instances of BoundingFrustum are not equal.
        /// </summary>
        /// <param name="a">The BoundingFrustum to the left of the inequality operator.</param>
        /// <param name="b">The BoundingFrustum to the right of the inequality operator.</param>
        public static bool operator !=(BoundingFrustum a, BoundingFrustum b)
        {
            return !(a == b);
        }
        /// <summary>
        /// Determines whether two instances of BoundingFrustum are equal.
        /// </summary>
        /// <param name="a">The BoundingFrustum to the left of the equality operator.</param>
        /// <param name="b">The BoundingFrustum to the right of the equality operator.</param>
        public static bool operator ==(BoundingFrustum a, BoundingFrustum b)
        {
            if (object.Equals(a, null))
                return (object.Equals(b, null));

            if (object.Equals(b, null))
                return (object.Equals(a, null));

            return a.Matrix == (b.Matrix);
        } 
        #endregion
    }
}
